#include "FreeRTOS.h"
#include "task.h"
#include "cmsis_os.h"
	

#include "locate_task.h"
#include "uart.h"


extern uint8_t x_signal;
extern uint8_t x_h ;
extern uint8_t x_l;
extern uint8_t y_signal;
extern uint8_t y_h ;
extern uint8_t y_l;
extern uint8_t angle_signal;
extern uint8_t angle_h;
extern uint8_t angle_l;

float X;
float Y;
float angle;

float Z;

//用于自瞄的PID

static float  EZ_pid(float angle)
{
	float out;
	
	out = angle * 0.1f;
	return out;
}

//获取雷达位置数据
static void get_position()
{
		X = (x_h << 8) | x_l;   			//车相对于轮框的X坐标
		Y = (y_h << 8) | y_l;				//车相对于轮框的Y坐标
		angle = (angle_h << 8) | angle_l;   //车相对于轮框的angle坐标
		
		if(!x_signal) X=-X;					
		if(!y_signal) Y=-Y;
		if(!angle_signal) angle=-angle;
}



extern "C" void LocateTask(void *argument);
osThreadId_t	LocateTaskHandle;
osThreadAttr_t LocateTask_attributes;

extern "C" void locate_task_init(void)
{

    {
        LocateTask_attributes.name = "LocateTask";
        LocateTask_attributes.stack_size = 128 * 4;
        LocateTask_attributes.priority = (osPriority_t) osPriorityNormal;
    };

	LocateTaskHandle = osThreadNew (LocateTask , NULL, &LocateTask_attributes  );
}


extern "C" void LocateTask(void *argument)
{

    osDelay(500);   //初始化后先延时500ms
	for(;;)
	{
		get_position();
		Z = EZ_pid(angle);
		osDelay(1);
	}


}

